#include "parafluidsegment.h"
#include "parafluidsmoothingkernel.h"
#include "parafluidsphposition.h"
#include "parafluidsphneighbor.h"
#include "parafluidtrianglevoxelizer.h"
#include "parafluidvector.h"

#include <QtDebug>
#include <QList>
#include <QSet>

#include <cmath>
#include <limits>

using namespace ParaFluid;

PositionUpdater::~PositionUpdater()
{
	/* does nothing */
}

PositionUpdater::PositionUpdater()
	: AtomVoxelizer::ReadWriteVisitor()
	, _SPH(0)
{
	/* does nothing */
}

void PositionUpdater::updatePosition(SPH & sph)
{
	_SPH = &sph;
	_SPH->atomVoxelizer.acceptVisitor(*this);
}

void PositionUpdater::updatePositionParallel(SPH & sph, int indexFirst, int indexLast)
{
	_SPH = &sph;
	_SPH->atomVoxelizer.acceptVisitor(*this, indexFirst, indexLast);
}

void applyConstraintsRecursive(int recursionLimit, SPH & sph, Atom & atom, const Triangle * lastIntersected, const Vector & lastIntersection)
{
	class IntersectionFinder
		: public TriangleVoxelizer::ReadOnlyVisitor
	{
		
	private:
		
	public:
		
		bool found;
		Segment segment;
		Vector intersection;
		const Triangle * lastIntersectedTriangle;
		const Triangle * intersectedTriangle;
		float distance;
		
		IntersectionFinder(const Segment & segmentToTest, const Triangle * lastIntersected)
			: found(false)
			, segment(segmentToTest)
			, intersection()
			, lastIntersectedTriangle(lastIntersected)
			, intersectedTriangle(0)
			, distance(std::numeric_limits<float>::infinity())
		{
			/* does nothing */
		}
		
		void visitUnique(const Triangle * triangle)
		{
			if(triangle != lastIntersectedTriangle)
			{
				Vector point;
				
				if(triangle->intersects(segment, point))
				{
					float newDistance = segment.pointA.distanceTo(point);
					
					if(newDistance < distance)
					{
						distance = newDistance;
						intersection = point;
						intersectedTriangle = triangle;
						found = true;
					}
				}
			}
		}
		
	} intersector(Segment(lastIntersection, atom.currentPosition), lastIntersected);
	
	// Visit all Segment Voxels
	QList<Vector> voxels = intersector.segment.visitedVoxels(sph.smoothingDistance());
	for(QList<Vector>::iterator i = voxels.begin(); i != voxels.end(); ++i)
	{
		sph.triangleVoxelizer.acceptVisitor(intersector, *i);
	}
	
	if(intersector.found)
	{
		/*
		// Simplistic approach
		Vector temp = atom.previousPosition;
		atom.previousPosition = atom.currentPosition;
		atom.currentPosition = temp;
		return;
		*/
		
		// Retrieve values
		Segment segment = intersector.segment;
		Vector point = intersector.intersection;
		const Triangle * triangle = intersector.intersectedTriangle;
		
		// Plane
		float a = triangle->normal().x;
		float b = triangle->normal().y;
		float c = triangle->normal().z;
		float d = -1 * (
			a * triangle->point1().x +
			b * triangle->point1().y +
			c * triangle->point1().z
		);
		
		// Compute Distance between Segment and Triangle Plane
		float distancePlanePointA =
			(a * segment.pointA.x) +
			(b * segment.pointA.y) +
			(c * segment.pointA.z) +
			d
		;
		float distancePlanePointB =
			(a * segment.pointB.x) +
			(b * segment.pointB.y) +
			(c * segment.pointB.z) +
			d
		;
		
		// Reflect Segment onto the Plane
		Vector reflectedPointA = segment.pointA - (triangle->normal() * (distancePlanePointA * 2));
		Vector reflectedPointB = segment.pointB - (triangle->normal() * (distancePlanePointB * 2));
		
		// Set Atom Position
		atom.previousPosition = reflectedPointA;
		atom.currentPosition = reflectedPointB;
		
		if(recursionLimit > 0)
		{
			applyConstraintsRecursive(--recursionLimit, sph, atom, triangle, point);
		}
		else
		{
			qDebug("Recursion failed");
		}
		
	}
}

void applyConstraints(SPH & sph, Atom & atom)
{
	applyConstraintsRecursive(5, sph, atom, 0, atom.previousPosition);
}

void PositionUpdater::visit(Atom & atom)
{
	// Apply Integrator
	Vector acceleration = _SPH->gravity();
	float timeStepPow2 = _SPH->timeStep() * _SPH->timeStep();
	if(atom.currentDensity != 0)
	{
		acceleration += atom.currentForce * (1.0 / atom.currentDensity);
	}
	atom.nextPosition.x = (2.0 * atom.currentPosition.x) - atom.previousPosition.x + (acceleration.x * timeStepPow2);
	atom.nextPosition.y = (2.0 * atom.currentPosition.y) - atom.previousPosition.y + (acceleration.y * timeStepPow2);
	atom.nextPosition.z = (2.0 * atom.currentPosition.z) - atom.previousPosition.z + (acceleration.z * timeStepPow2);
	
	atom.previousPosition = atom.currentPosition;
	atom.currentPosition = atom.nextPosition;
	
	applyConstraints(*_SPH, atom);
}
